#include <hidef.h>      /* common defines and macros */
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "derivative.h"      /* derivative-specific definitions */
#include "i2c.h"
#include "sci.h"
#include "pll.h"
#include "lcd.h"
#include "pwm.h"
#include "atd.h"
#include "dac.h"

#define PI 3.14159
#define BLANK "                "
#define IMAX      1000
#define IMIN      -1000
#define MAXJOY    30767
#define MAXDAC    255
#define MODE_GPS 1
#define MODE_JOY 0


static volatile int RTIcycles,Disp;
static volatile double latitude,longitude,bearingGPS,DOP,speed,latT,longT,DesiredSteering,DesiredVelocity,ActualSteering,Kp,Kd,Ki,lastE,iE,counter,tempa,tempd,C;
static volatile int X,Y,Trig,cmdMode,MINATD,MAXATD;
static volatile unsigned char SCI0Index = 0;
static volatile unsigned char SCI0Temp[MAXLENGTH] = {0};
static volatile unsigned char SCI0Buff[MAXLENGTH] = {0};

static volatile unsigned char SCI1Index = 0;
static volatile unsigned char BuffSelect = 0;
static volatile unsigned char SCI1Temp[90] = {0};
static volatile unsigned char GSA[90] = {0};
static volatile unsigned char RMC[90] = {0};

void parse_GPS(void);
void parse_command(void);

void calc_gps_angle(double,double);
void calc_joy_angle(void);
void calc_steering_angle(void);
void steer(void);
void drive(void);

void main(void) {
  char val[30]={0};		//temp value for sprintf
  latitude=35.4517209;
  longitude=-97.5366210;
  latT=36.162122;
  longT=-95.971069;
  lastE=0;
  iE=0;
  cmdMode=MODE_JOY;
  RTIcycles = 0;
  MINATD=200;
  MAXATD=40;
  Disp=0;
  Kd=1;
  Ki=0;
  Kp=0.2;

  /* Setup Control Registers */
  RTICTL=0x33;
  CRGINT |= 0x80;
  PIEH=0x01;
  PPSH=0x00;
              
  EnableInterrupts; 
  	
  PLL_Init();
  sci_init();
  i2c_init();
  LCD_init();
  pwm_init();
  atd_init();
  dac_init();
    
  for(;;) {
    if(RTIcycles>128) {      
     if (Disp==0) {
      sprintf(val,"COMP:%f        ",(double)i2c_getAngle()/10);
      writeLine(val,0);
      writeLine(BLANK,1);
     } else if (Disp==1) {
      sprintf(val,"Lat:%f",latitude);
      writeLine(val,0);
      sprintf(val,"Lon:%f",longitude);
      writeLine(val,1);
     } else if (Disp==2) {
      sprintf(val,"X:%d Y:%d         ",X,Y);
      writeLine(val,0);
      sprintf(val,"Trig:%d          ",Trig);
      writeLine(val,1);
     } else if (Disp==3) {
      sprintf(val,"%f,%f",DesiredSteering,ActualSteering);
      writeLine(val,0);
      sprintf(val,"%d,%f",atd_getValue(3),C);
      writeLine(val,1);
     } else {
      sprintf(val,"a=%f              ",tempa);
      writeLine(val,0);
      sprintf(val,"dist=%f            ",tempd);
      writeLine(val,1);
     }
     RTIcycles=0;
    } 
    if(RTIcycles%16==0 && RTIcycles!=0) {
      calc_steering_angle();
      if (cmdMode==MODE_GPS) {
        calc_gps_angle(latT,longT);
      } else if (cmdMode==MODE_JOY) {
        calc_joy_angle();
      }
      steer();
      if (Trig!=1) {
      	drive();
      } else {
      	pwm_set(17,1);			//HIT THE BRAKES!
      }
    }

  } /* loop forever */
  /* please make sure that you never leave main */
}

void parse_GPS(void) {
  int i;
  strtok(GSA,",");
  for(i=0;i<12;i++) {
    strtok(NULL,",");
  }
  DOP=atof(strtok(NULL,","));
  strtok(RMC,",");
  strtok(NULL,",");
  strtok(NULL,",");
  latitude=atof(strtok(NULL,","))/100;
  strtok(NULL,",");
  longitude=atof(strtok(NULL,","))/100;
  strtok(NULL,",");
  speed=atof(strtok(NULL,","));
  bearingGPS=atof(strtok(NULL,","));
}

void parse_command(void) {
  char *mode;
  mode=strtok(SCI0Buff,",");
  if (strcmp("GPS",mode)==0) {
    cmdMode=MODE_GPS;
    latT=atof(strtok(NULL,","));
    longT=atof(strtok(NULL,","));
    Trig=atoi(strtok(NULL,","));
  } else if (strcmp("JOY",mode)==0) {
    cmdMode=MODE_JOY;
    X=atoi(strtok(NULL,","));
    Y=atoi(strtok(NULL,","));
    Trig=atoi(strtok(NULL,","));
  } else if (strcmp("PID",mode)==0){
    Kp=atof(strtok(NULL,",")); 
    Ki=atof(strtok(NULL,","));
    Kd=atof(strtok(NULL,","));
  } else if (strcmp("POT",mode)==0){
    MINATD=atoi(strtok(NULL,",")); 
    MAXATD=atoi(strtok(NULL,","));
  }
  counter=0;				//reset timeout killswitch
}

void calc_gps_angle(double latD, double lonD) {
 double a, b, c, d, dL, dLo, toRad,desHead,wheelHead;
 double latS, longS;
 toRad = PI/180;
 dL = (latD - latitude) * toRad;
 dLo = (lonD- longitude) * toRad;
 latD = (latD) * toRad;
 lonD = lonD * toRad;
 latS = (latitude) * toRad;
 longS = longitude * toRad;
 //c = 2*asin(sqrt(sin(dL/2)*sin(dL/2) + cos(latS)*cos(latD)*sin(dLo/2)*sin(dLo/2)));
 a = sin(dL/2)*sin(dL/2) + sin(dLo/2)*sin(dLo/2)*cos(latS)*cos(latD);
 c = 2*atan2(sqrt(a),sqrt(1-a));
 b = atan2((sin(dLo)*cos(latD)),(cos(latS)*sin(latD) - sin(latS)*cos(latD)*cos(dLo)));
 d = 6371 * c;
 tempd = d;
 tempa = b;
 a=b;
  if (a<=PI) {
    desHead=a+PI;     //desired heading
  } else {
    desHead=a-PI;
  }
  wheelHead=(((double)i2c_getAngle()/10)-ActualSteering)*PI/180;
  
  if ((desHead-wheelHead)*180/PI < -180){
    DesiredSteering = -180;
  } else if ((desHead-wheelHead)*180/PI > 180) {
    DesiredSteering = 180;
  } else {
    DesiredSteering = (desHead-wheelHead)*180/PI;
  }
}

void calc_joy_angle() {
  DesiredSteering = (double)X*180/MAXJOY;		//center angles around zero, scale to 180
  if (Y <= 0) {
    DesiredVelocity = -(double)Y*MAXDAC/MAXJOY;
  } else {
    DesiredVelocity = 0;
  }
}

void calc_steering_angle() {
  double val01,atdspan,anglespan;
  atdspan=MAXATD-MINATD;
  anglespan=360;
  val01=(atd_getValue(3)-MINATD)/atdspan;
  ActualSteering=-180 + (val01*anglespan); //center angles around 0, scale to 180
}

void steer() {
  double thisE,pE,dE,iTerm,cTerm;
  int c;
  thisE=DesiredSteering-ActualSteering;		//Error Signal
  pE=thisE*Kp;					//Proportional Component
  iE+=thisE;					
  if (iE > IMAX) {
    iE=IMAX;
  } else if (iE < IMIN) {
    iE=IMIN;
  }
  iTerm=iE*Ki;					//Integral Component
  dE=(thisE-lastE)*Kd;				//Derivative Component
  cTerm=pE+iTerm+dE;				//PID Control Signal

  if (cTerm/10 > 6) {
    c=6;			//max pwm duty
  } else if (cTerm/10 < -6) {
    c=-6;			//min pwm duty
  } else {
    c=(int)(cTerm/10);		//scaled control signal
  }
  pwm_set(18-c,0);		//Neutral at 1.5ms
  lastE=thisE;
  C=cTerm; 
}

void drive() {
  if (Y<=0) {
    pwm_set(23+(Y/5000),1);
  } else {
    pwm_set(23,1);
  }
}

__interrupt void RealTimeInterrupt(void)
{
  RTIcycles++;                    /* increment the counter        */
  CRGFLG = 0x80;            /* clear rti flag               */
}

__interrupt void SCI0Interrupt(void)
{
   unsigned char c;
   
   c=SCI0SR1;
   c=SCI0DRL;
   if (c != 'E') {           //Not end of string
     SCI0Temp[SCI0Index]=c;   //Append character received
     SCI0Index++;
   } else {                   //End of string
     SCI0Temp[SCI0Index]=c;
     copystr(SCI0Temp,SCI0Buff,MAXLENGTH);  //Copy received string from worKing to read buffer
     //sci_print(SCI0Buff,1);   // For testing
     SCI0Index = 0;
     parse_command();              
     zero(SCI0Temp,MAXLENGTH); //Reset and zero out worKing buffer
   }
}


__interrupt void SCI1Interrupt(void)
{
   unsigned char c;
   c=SCI1SR1;
   c=SCI1DRL;
   if (c != 13) {             //Not end of string
     SCI1Temp[SCI1Index]=c;     //Append character received
     SCI1Index++;
   } else {                      //End of string
     if (BuffSelect == 0) {
       copystr(SCI1Temp,GSA,90);  //Copy received string from worKing to GSA buffer
     } else {
       copystr(SCI1Temp,RMC,90);  //Copy received string from worKing to RMC buffer
       parse_GPS();
     }
     SCI1Index = 0;
     BuffSelect = 1 - BuffSelect;  //Toggle Buffer
     zero(SCI1Temp,90);  //Reset and zero out worKing buffer
   }
}

__interrupt void PortHInterrupt(void) {
   while(PTH==0);
   if(Disp>=4) {
      Disp=0;
   } else {
      Disp++;
   }
   PIFH=0xFF;
}